import rospy
import time
from geometry_msgs.msg import PoseStamped

if __name__ == '__main__':
    rospy.init_node('pubpose')
    turtle_vel_pub = rospy.Publisher('mavros/vision_pose/pose', PoseStamped, queue_size=1)
    
    mypose=PoseStamped()
    turtle_vel_pub.publish(mypose) #先发送一个空位置，试探一下，否则第一个包容易丢
    time.sleep(1)
    rate = rospy.Rate(50)
    mypose=PoseStamped()
    dx = 0.01
    while True:
        mypose.header.stamp = rospy.Time.now()
        mypose.header.frame_id='' #设置自己的目标
        mypose.pose.position.x=dx
        mypose.pose.position.y=dx
        mypose.pose.position.z=dx
        mypose.pose.orientation.x=0
        mypose.pose.orientation.y=0
        mypose.pose.orientation.z=1
        mypose.pose.orientation.w=0
        
        turtle_vel_pub.publish(mypose) #发送自己设置的目标点
        dx = dx + 0.01
        rate.sleep()